#include<iostream>
#include "matlite.h"
#include <math.h>

using namespace std;



//frmIdx[]={rowstart, colstart, rowend, colend}, inclusive indexing
//toIdx[]={rowstart, colstart}
//copyRegion from a to parent
//no border check, so take care!!!
void matlite::copyRegion(matlite &a, int frmIdx[], int toIdx[])
{
  for(int i=frmIdx[0], ii=toIdx[0]; i<=frmIdx[2]; i++, ii++){
    for(int j=frmIdx[1], jj=toIdx[1]; j<=frmIdx[3]; j++, jj++){
      val[ii][jj]=a.val[i][j];
    }
  }
}



matlite::matlite()
{
  m_nrow=m_ncol=m_rsrow=m_rscol=0; 
  m_rowinc=m_colinc=0;
}


matlite::matlite(int nrow, int ncol)
{
  m_rowinc=m_colinc=0;
  m_nrow=m_rsrow=nrow;
  m_ncol=m_rscol=ncol;

  val=new double* [m_rsrow];
  for(int i=0; i<m_rsrow; i++)
    val[i]=new double [m_rscol];
}

matlite::~matlite()
{
  for(int i=0; i<m_rsrow; i++)
    delete [] val[i];
  if(m_rsrow > 0) delete [] val;
  m_nrow=m_ncol=m_rsrow=m_rscol=0;
  m_rowinc=m_colinc=10;
}

matlite::matlite(matlite &src)
{
  m_rowinc=m_colinc=0;
  m_nrow=m_rsrow=src.nRow();
  m_ncol=m_rscol=src.nCol();

  val=new double* [m_rsrow];
  for(int i=0; i<m_rsrow; i++)
    val[i]=new double [m_rscol];

  for(int i=0; i<src.nRow(); i++){
    for(int j=0; j<src.nCol(); j++){
      val[i][j]=src.val[i][j];
    }
  }

}




void matlite::resize(int nrow, int ncol, bool isKeepData)
{

  //same size, no change
  if(nrow==m_nrow && ncol==m_ncol) return;
  
  //larger than reserve
  //copyRegion to new block of memory
  if(nrow>m_rsrow || ncol>m_rscol) {
    int oldrsrow=m_rsrow;

    if(nrow>m_rsrow) {
      m_rsrow=nrow+m_rowinc;
      m_rowinc=10;
    }else m_rsrow=nrow;
    if(ncol>m_rscol) {
      m_rscol=ncol+m_colinc;
      m_colinc=10;
    }else m_rscol=ncol;

    //create new memory block
    double **newspace=new double* [m_rsrow];
    for(int i=0; i<m_rsrow; i++){
      newspace[i]=new double [m_rscol];}

    //copyRegion to the new block
    if(isKeepData){
      for(int i=0; i<m_nrow; i++){
	for(int j=0; j<m_ncol; j++){
	  newspace[i][j]=val[i][j];
	}
      }
    }
    //dealloc the old block
    for(int i=0; i<oldrsrow; i++){
      delete [] val[i];}
    if(oldrsrow > 0) delete [] val;

    val=newspace;
    m_nrow=nrow;
    m_ncol=ncol;

  }else{
    //else just adjust the row and column indices will do
    m_nrow=nrow;
    m_ncol=ncol;
  }

}

void matlite::zeros(int nrow, int ncol)
{
  resize(nrow,ncol);
  for(int i=0; i<nrow; i++){
    for(int j=0; j<ncol; j++){
      val[i][j]=0;
}}
}

void matlite::eye(int nrow, int ncol)
{
  resize(nrow,ncol);
  for(int i=0; i<nrow; i++){
    for(int j=0; j<ncol; j++){
      val[i][j]=0;
  }}

  int nn=(nrow<ncol)?nrow:ncol;
  for(int i=0; i<nn; i++){
    val[i][i]=1;
}
}

void matlite::show()
{
  cout<<"dim: "<<m_nrow<<"x"<<m_ncol;
  cout<<"  reserve: "<<m_rsrow<<"x"<<m_rscol<<endl;
  for(int i=0; i<m_nrow; i++){
    for(int j=0; j<m_ncol; j++){
      cout<<val[i][j]<<"   ";
    }
    cout<<endl;
  }
  cout<<endl;

}

void matAdd(matlite &ans, matlite &a, matlite &b)
{
  if(a.m_nrow!=b.m_nrow || a.m_ncol!=b.m_ncol){
    cerr<<"matAdd error: unmatched input matrix size"<<endl;
    return;
  }
  int nrow=a.m_nrow, ncol=a.m_ncol;
  ans.resize(nrow,ncol);
  
  for(int i=0; i<nrow; i++){
    for(int j=0; j<ncol; j++){
      ans.val[i][j]=a.val[i][j]+b.val[i][j];
     }
  }
}

void matMinus(matlite &ans, matlite &a, matlite &b)
{
  if(a.m_nrow!=b.m_nrow || a.m_ncol!=b.m_ncol){
    cerr<<"matAdd error: unmatched input matrix size"<<endl;
    return;
  }
  int nrow=a.m_nrow, ncol=a.m_ncol;
  ans.resize(nrow,ncol);
  
  for(int i=0; i<nrow; i++){
    for(int j=0; j<ncol; j++){
      ans.val[i][j]=a.val[i][j]-b.val[i][j];
    }
  }
}


void transpose(matlite &ans,matlite &trans){

  ans.resize(trans.nCol(),trans.nRow());
  for(int i=0;i<trans.nCol();i++){
    for(int j=0;j<trans.nRow();j++){
      ans.val[i][j]=trans.val[j][i];
    }
   }
}


void matMult(matlite &ans, matlite &a, matlite &b, bool isTransposeB)
{
  if(!isTransposeB){
    //A*B
    if(a.m_ncol != b.m_nrow){
      cerr<<"matMult error: nColumn of A != nRow of B"<<endl;
      return;    
    }
    int nr1=a.m_nrow, nc1=a.m_ncol, /*nr2=b.m_nrow,*/ nc2=b.m_ncol;
    ans.resize(nr1,nc2);

    for(int i=0; i<nr1; i++){
      for(int k=0; k<nc2; k++){
	double sum=0.0;
	for(int j=0; j<nc1; j++){
	  sum+=a.val[i][j]*b.val[j][k];
	}
	ans.val[i][k]=sum;
      }
    }
  }else{
    //A*B'
    if(a.m_ncol != b.m_ncol){
      cerr<<"matMult error: nColumn of A != nRow of B'"<<endl;
      return;    
    }
    int nr1=a.m_nrow, nc1=a.m_ncol, nr2=b.m_nrow /*,nc2=b.m_ncol*/;
    ans.resize(nr1,nr2);

    for(int i=0; i<nr1; i++){
      for(int k=0; k<nr2; k++){
	double sum=0.0;
	for(int j=0; j<nc1; j++){
	  sum+=a.val[i][j]*b.val[k][j];
	}
	ans.val[i][k]=sum;
      }
    }
  }
  

}

void matMultAdd(matlite &ans, matlite &a, matlite &b, matlite &c, bool isTransposeB)
{
  matMult(ans, a,b, isTransposeB);
  
  if(ans.m_nrow!=c.m_nrow || ans.m_ncol!=c.m_ncol){
    cerr<<"matMultAdd error: unmatched input matrix size"<<endl;
    return;
  }
  int nrow=ans.m_nrow, ncol=ans.m_ncol;
  
  for(int i=0; i<nrow; i++)
    for(int j=0; j<ncol; j++)
      ans.val[i][j]+=c.val[i][j];

}

bool matlite::checkPositiveDef()
{
  int n=m_nrow, k;
  double sum;
  for(int i=0; i<n; i++){
    for(int j=i; j<n; j++){
      for(sum=val[i][j], k=i-1; k>=0; k--) sum-=val[i][k]*val[j][k];
      if(i==j){
	if(sum <= 0.0) {//not seem to positve definite
	  return false; 
	}
      }
    }
  }
  return true;

}

//only applicable for inverse of square, symmetric, postive definiteness matrix
//however, it is very stable in preserving this characteristics after the op.
void matlite::choleskyInv()
{
  if(m_nrow!=m_ncol){
    cerr<<"choleskyInv error: matrix not square"<<endl;
    return;
  }

  //Modified from Cholesky Decomposition from Numerical Recipe in C, Ch 2.9
  //Given a positive-definite symmetric matrix, this routine constructs its
  //Cholesky decomposition A=L*L'.  The Cholesky factor L overwrites the 
  //lower half (including the diag of the original matrix.
  int n=m_nrow, k;
  double sum;
  double* diag=new double[n];
  for(int i=0; i<n; i++){
    for(int j=i; j<n; j++){
      for(sum=val[i][j], k=i-1; k>=0; k--) sum-=val[i][k]*val[j][k];
      if(i==j){
	if(sum <= 0.0) {//not seem to positve definite
	  cerr<<"choleskyInv error: matrix not positive definite"<<endl;
	}
	diag[i]=sqrt(sum);
      }else{
	val[j][i]=sum/diag[i];
      }
    }
  }
  for(int i=0; i<n; i++) val[i][i]=diag[i];

  //calculate the inverse of A by back substitution
  //D=L \ I;  inv(A)=L' \D
  double **tmp=new double* [n];
  for(int i=0; i<n; i++)
    tmp[i]=new double [n+1]; //s.t. it can store 2 half triangles

  //D is a lower triangle, stored at the lower triangle of tmp
  for(int i=0; i<n; i++) tmp[i][i]=1.0/diag[i];
  //calc the rest
  for(int i=0; i<n; i++){
    for(int j=0; j<i; j++){
      for(sum=0.0, k=j; k<i; k++) {
	sum+=val[i][k]*tmp[k][j];
      }
      tmp[i][j]=-sum/val[i][i];
    }
  }

  //transpose L and copyRegion that to the upper triangle of tmp
  for(int i=0; i<n; i++){
    for(int j=i; j<n; j++){
      tmp[i][j+1]=val[j][i];
    }
  }


  //put the inverse back to the original matrix, inv(A)=L' \D
  //if U=L'
  //   for(int i=n-1; i>=0; i--) {
  //     for(int j=0; j<=i; j++){
  //       for(sum=0.0, k=i+1; k<n; k++) sum+=u[i][k]*val[k][j];
  //       if(i==j) diag[i]=(d[i][j]-sum)/u[i][i];
  //       else val[i][j]=(d[i][j]-sum)/u[i][i];
  //     }
  //   }
  //however, the lower triangle of the origianl is still L,
  //need some hack to save creating a separated working matrix
  for(int i=n-1; i>=0; i--) {
    for(int j=0; j<=i; j++){
      for(sum=0.0, k=i+1; k<n; k++) sum+=tmp[i][k+1]*val[k][j];
      if(i==j) diag[i]=(tmp[i][j]-sum)/tmp[i][i+1];
      else val[i][j]=(tmp[i][j]-sum)/tmp[i][i+1];
      val[i][j]=(tmp[i][j]-sum)/tmp[i][i+1];
    }
  }
  for(int i=0; i<n; i++) val[i][i]=diag[i];

  //since the inverse of a postive definite matrix is also symmetric,
  //we only need to calc one half... and mirror the rest as below
  for(int i=0; i<n; i++){
    for(int j=i+1; j<n; j++){
      val[i][j]=val[j][i];
    }
  }

  for(int i=0; i<n; i++)
    delete [] tmp[i];
  delete [] tmp;
  delete [] diag;
}


void matinv(matlite &Return,matlite Inv){

	double multiplier;
	matlite I;
	I.eye(Inv.nRow(),Inv.nCol());
	Return.resize(Inv.nRow(),Inv.nCol());

	for(int i=0;i<Inv.nRow();i++){
		if (Inv.val[i][i] != 1){
			multiplier = 1/Inv.val[i][i];
		}

		else{
			multiplier = 1;
		}	
		
		for (int j=0;j<Inv.nCol();j++){
			Inv.val[i][j] = Inv.val[i][j]*multiplier;
			I.val[i][j] = I.val[i][j]*multiplier;
		}

		for(int k=0;k<Inv.nRow();k++){
			if (k!=i){
				if ((Inv.val[k][i]-Inv.val[i][i])!=0){
					multiplier = Inv.val[k][i]/Inv.val[i][i];
				}
				else{
					multiplier = 1;
				}	
				for (int j=0;j<Inv.nCol();j++){
					Inv.val[k][j] = Inv.val[k][j] - Inv.val[i][j]*multiplier;
					I.val[k][j] = I.val[k][j] - I.val[i][j]*multiplier;
				}
			}
		}
		
	}
	for(int i=0;i<Inv.nRow();i++){
		for(int j=0;j<Inv.nCol();j++){
			Return.val[i][j]=I.val[i][j];
		}
	}
}

// int main()
// {
//   matlite ma,mb(4,3),mc;
//   ma.resize(3,4);

//   ma.val[0][0]=1;   ma.val[0][1]=3;   ma.val[0][2]=9;   ma.val[0][3]=4; 
//   ma.val[1][0]=-2;   ma.val[1][1]=0;   ma.val[1][2]=1;   ma.val[1][3]=2; 
//   ma.val[2][0]=1;   ma.val[2][1]=2;   ma.val[2][2]=1;   ma.val[2][3]=0;

//   mb.val[0][0]=3; mb.val[0][1]=2; mb.val[0][2]=4;
//   mb.val[1][0]=5; mb.val[1][1]=8; mb.val[1][2]=7;
//   mb.val[2][0]=0; mb.val[2][1]=2; mb.val[2][2]=-2;
//   mb.val[3][0]=2; mb.val[3][1]=4; mb.val[3][2]=3;

//   mc.eye(3,3);
  
//   matlite t1,t2;
//   matMult(t1,ma,mb);
//   cout<<"a*b ";
//   t1.show();

//   matMult(t1,ma,ma,true);
//   cout<<"a*a' ";
//   t1.show();


//   matMultAdd(t2,ma,mb,mc);
//   cout<<"a*b+c ";
//   t2.show();

//   mc.resize(3,4,true);
//   mc.val[0][3]=3;
//   mc.val[1][3]=-2;
//   mc.val[2][3]=4;

//   matAdd(t2,ma,mc);
//   cout<<"a+d ";
//   t2.show();

//   mb.resize(3,3);
//   mb.val[0][0]=107; mb.val[0][1]=15; mb.val[0][2]=16;
//   mb.val[1][0]=15;  mb.val[1][1]=9;  mb.val[1][2]=-1;
//   mb.val[2][0]=16;  mb.val[2][1]=-1; mb.val[2][2]=6;

//   mb.choleskyInv();
//   cout<<"chol(b) ";
//   mb.show();

//   int src[]={0,0,1,1}, target[]={0,0};
//   mb.copyRegion(ma,src,target);
//   cout<<"copyRegion to b";
//   mb.show();
//   return 0;
  
// }

